<!DOCTYPE HTML PUBLIC "-//W3C//DTD HTML 4.01 Transitional//EN"
                "http://www.w3.org/TR/REC-html40/loose.dtd">
<html>
<head>
  <title>Description of srukf</title>
  <meta name="keywords" content="srukf">
  <meta name="description" content="SRUKF  Square Root Unscented Kalman Filter (Sigma-Point Kalman Filter variant)">
  <meta http-equiv="Content-Type" content="text/html; charset=iso-8859-1">
  <meta name="generator" content="m2html &copy; 2003 Guillaume Flandin">
  <meta name="robots" content="index, follow">
  <link type="text/css" rel="stylesheet" href="../../m2html.css">
</head>
<body>
<a name="_top"></a>
<div><a href="../../menu.html">Home</a> &gt;  <a href="#">ReBEL-0.2.7</a> &gt; <a href="#">core</a> &gt; srukf.m</div>

<!--<table width="100%"><tr><td align="left"><a href="../../menu.html"><img alt="<" border="0" src="../../left.png">&nbsp;Master index</a></td>
<td align="right"><a href="menu.html">Index for .\ReBEL-0.2.7\core&nbsp;<img alt=">" border="0" src="../../right.png"></a></td></tr></table>-->

<h1>srukf
</h1>

<h2><a name="_name"></a>PURPOSE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>SRUKF  Square Root Unscented Kalman Filter (Sigma-Point Kalman Filter variant)</strong></div>

<h2><a name="_synopsis"></a>SYNOPSIS <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="box"><strong>function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srukf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS) </strong></div>

<h2><a name="_description"></a>DESCRIPTION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre class="comment"> SRUKF  Square Root Unscented Kalman Filter (Sigma-Point Kalman Filter variant)

   [xh, Sx, pNoise, oNoise, InternalVariablesDS] = SRUKF(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)

   This filter assumes the following standard state-space model:

     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]
     y(k) = hfun[x(k),n(k),U2(k)]

   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state
   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the
   system.

   INPUT
         state                state mean at time k-1          ( xh(k-1) )
         Sstate               lower triangular Cholesky factor of state covariance at time k-1    ( Sx(k-1) )
         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')
         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')
         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )
         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )
         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )
         InferenceDS          SPOK inference data structure generated by GENINFDS function.

   OUTPUT
         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )
         Sx                   Cholesky factor of state covariance at time k  ( Sx(k) )
         pNoise               process noise data structure     (possibly updated)
         oNoise               observation noise data structure (possibly updated)

         InternalVariablesDS  (optional) internal variables data structure
            .xh_                 predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )
            .Sx_                 predicted state covariance (Cholesky factor)
            .yh_                 predicted observation ( E[y(k)|Y(k-1)] )
            .inov                inovation signal
            .Pinov               inovation covariance
            .KG                  Kalman gain

   Required InferenceDS fields:
         .spkfParams           SPKF parameters = [alpha beta kappa] with
                                   alpha  :  UKF scale factor
                                   beta   :  UKF covariance correction factor
                                   kappa  :  UKF secondary scaling parameter
   Copyright (c) Oregon Health &amp; Science University (2006)

   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for
   academic use only (see included license file) and can be obtained from
   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the
   software should contact rebel@csee.ogi.edu for commercial licensing information.

   See LICENSE (which should be part of the main toolkit distribution) for more
   detail.</pre></div>

<!-- crossreference -->
<h2><a name="_cross"></a>CROSS-REFERENCE INFORMATION <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
This function calls:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li><li><a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>	CVECREP  Column vector replicate</li><li><a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>	ADDANGLE   Addition function for 'angle space' sigma-points expressed in radians.</li></ul>
This function is called by:
<ul style="list-style-image:url(../../matlabicon.gif)">
<li><a href="gmsppf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise, extra] = gmsppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">gmsppf</a>	GMSPPF  Gaussian Mixture Sigma-Point Particle Filter</li><li><a href="sppf.html" class="code" title="function [estimate, ParticleFilterDS, pNoise, oNoise] = sppf(ParticleFilterDS, pNoise, oNoise, obs, U1, U2, InferenceDS)">sppf</a>	SPPF  Sigma-Point Particle Filter.</li><li><a href="../.././ReBEL-0.2.7/examples/joint_estimation/demje1.html" class="code" title="">demje1</a>	DEMJE1  Demonstrate joint estimation on a 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/parameter_estimation/dempe1.html" class="code" title="">dempe1</a>	DEMPE1  Demonstrate parameter estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse1.html" class="code" title="">demse1</a>	DEMSE1  Demonstrate state estimation on a simple 2nd order LTI system.</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse2.html" class="code" title="">demse2</a>	DEMSE2  Demonstrate state estimation on a simple scalar nonlinear (time variant) problem</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse3.html" class="code" title="">demse3</a>	DEMSE3  Demonstrate nonlinear time series state estimation for Mackey-Glass chaotic time series</li><li><a href="../.././ReBEL-0.2.7/examples/state_estimation/demse4.html" class="code" title="">demse4</a>	DEMSE4  Bearing Only Tracking Example</li></ul>
<!-- crossreference -->


<h2><a name="_source"></a>SOURCE CODE <a href="#_top"><img alt="^" border="0" src="../../up.png"></a></h2>
<div class="fragment"><pre>0001 <a name="_sub0" href="#_subfunctions" class="code">function [xh, Sx, pNoise, oNoise, InternalVariablesDS] = srukf(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</a>
0002 
0003 <span class="comment">% SRUKF  Square Root Unscented Kalman Filter (Sigma-Point Kalman Filter variant)</span>
0004 <span class="comment">%</span>
0005 <span class="comment">%   [xh, Sx, pNoise, oNoise, InternalVariablesDS] = SRUKF(state, Sstate, pNoise, oNoise, obs, U1, U2, InferenceDS)</span>
0006 <span class="comment">%</span>
0007 <span class="comment">%   This filter assumes the following standard state-space model:</span>
0008 <span class="comment">%</span>
0009 <span class="comment">%     x(k) = ffun[x(k-1),v(k-1),U1(k-1)]</span>
0010 <span class="comment">%     y(k) = hfun[x(k),n(k),U2(k)]</span>
0011 <span class="comment">%</span>
0012 <span class="comment">%   where x is the system state, v the process noise, n the observation noise, u1 the exogenous input to the state</span>
0013 <span class="comment">%   transition function, u2 the exogenous input to the state observation function and y the noisy observation of the</span>
0014 <span class="comment">%   system.</span>
0015 <span class="comment">%</span>
0016 <span class="comment">%   INPUT</span>
0017 <span class="comment">%         state                state mean at time k-1          ( xh(k-1) )</span>
0018 <span class="comment">%         Sstate               lower triangular Cholesky factor of state covariance at time k-1    ( Sx(k-1) )</span>
0019 <span class="comment">%         pNoise               process noise data structure     (must be of type 'gaussian' or 'combo-gaussian')</span>
0020 <span class="comment">%         oNoise               observation noise data structure (must be of type 'gaussian' or 'combo-gaussian')</span>
0021 <span class="comment">%         obs                  noisy observations starting at time k ( y(k),y(k+1),...,y(k+N-1) )</span>
0022 <span class="comment">%         U1                   exogenous input to state transition function starting at time k-1 ( u1(k-1),u1(k),...,u1(k+N-2) )</span>
0023 <span class="comment">%         U2                   exogenous input to state observation function starting at time k  ( u2(k),u2(k+1),...,u2(k+N-1) )</span>
0024 <span class="comment">%         InferenceDS          SPOK inference data structure generated by GENINFDS function.</span>
0025 <span class="comment">%</span>
0026 <span class="comment">%   OUTPUT</span>
0027 <span class="comment">%         xh                   estimates of state starting at time k ( E[x(t)|y(1),y(2),...,y(t)] for t=k,k+1,...,k+N-1 )</span>
0028 <span class="comment">%         Sx                   Cholesky factor of state covariance at time k  ( Sx(k) )</span>
0029 <span class="comment">%         pNoise               process noise data structure     (possibly updated)</span>
0030 <span class="comment">%         oNoise               observation noise data structure (possibly updated)</span>
0031 <span class="comment">%</span>
0032 <span class="comment">%         InternalVariablesDS  (optional) internal variables data structure</span>
0033 <span class="comment">%            .xh_                 predicted state mean ( E[x(t)|y(1),y(2),..y(t-1)] for t=k,k+1,...,k+N-1 )</span>
0034 <span class="comment">%            .Sx_                 predicted state covariance (Cholesky factor)</span>
0035 <span class="comment">%            .yh_                 predicted observation ( E[y(k)|Y(k-1)] )</span>
0036 <span class="comment">%            .inov                inovation signal</span>
0037 <span class="comment">%            .Pinov               inovation covariance</span>
0038 <span class="comment">%            .KG                  Kalman gain</span>
0039 <span class="comment">%</span>
0040 <span class="comment">%   Required InferenceDS fields:</span>
0041 <span class="comment">%         .spkfParams           SPKF parameters = [alpha beta kappa] with</span>
0042 <span class="comment">%                                   alpha  :  UKF scale factor</span>
0043 <span class="comment">%                                   beta   :  UKF covariance correction factor</span>
0044 <span class="comment">%                                   kappa  :  UKF secondary scaling parameter</span>
0045 <span class="comment">%   Copyright (c) Oregon Health &amp; Science University (2006)</span>
0046 <span class="comment">%</span>
0047 <span class="comment">%   This file is part of the ReBEL Toolkit. The ReBEL Toolkit is available free for</span>
0048 <span class="comment">%   academic use only (see included license file) and can be obtained from</span>
0049 <span class="comment">%   http://choosh.csee.ogi.edu/rebel/.  Businesses wishing to obtain a copy of the</span>
0050 <span class="comment">%   software should contact rebel@csee.ogi.edu for commercial licensing information.</span>
0051 <span class="comment">%</span>
0052 <span class="comment">%   See LICENSE (which should be part of the main toolkit distribution) for more</span>
0053 <span class="comment">%   detail.</span>
0054 
0055 <span class="comment">%=============================================================================================</span>
0056 
0057 Xdim  = InferenceDS.statedim;                                <span class="comment">% extract state dimension</span>
0058 Odim  = InferenceDS.obsdim;                                  <span class="comment">% extract observation dimension</span>
0059 U1dim = InferenceDS.U1dim;                                   <span class="comment">% extract exogenous input 1 dimension</span>
0060 U2dim = InferenceDS.U2dim;                                   <span class="comment">% extract exogenous input 2 dimension</span>
0061 Vdim  = InferenceDS.Vdim;                                    <span class="comment">% extract process noise dimension</span>
0062 Ndim  = InferenceDS.Ndim;                                    <span class="comment">% extract observation noise dimension</span>
0063 
0064 NOV = size(obs,2);                                           <span class="comment">% number of input vectors</span>
0065 
0066 <span class="comment">%------------------------------------------------------------------------------------------------------------------</span>
0067 <span class="comment">%-- ERROR CHECKING</span>
0068 
0069 <span class="keyword">if</span> (nargin ~= 8) error(<span class="string">' [ srukf ] Not enough input arguments.'</span>); <span class="keyword">end</span>
0070 
0071 <span class="keyword">if</span> (Xdim~=size(state,1)) error(<span class="string">'[ srukf ] Prior state dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0072 <span class="keyword">if</span> (Xdim~=size(Sstate,1)) error(<span class="string">'[ srukf ] Prior state covariance dimension differs from InferenceDS.statedim'</span>); <span class="keyword">end</span>
0073 <span class="keyword">if</span> (Odim~=size(obs,1)) error(<span class="string">'[ srukf ] Observation dimension differs from InferenceDS.obsdim'</span>); <span class="keyword">end</span>
0074 <span class="keyword">if</span> U1dim
0075   [dim,nop] = size(U1);
0076   <span class="keyword">if</span> (U1dim~=dim) error(<span class="string">'[ srukf ] Exogenous input U1 dimension differs from InferenceDS.U1dim'</span>); <span class="keyword">end</span>
0077   <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ srukf ] Number of observation vectors and number of exogenous input U1 vectors do not agree!'</span>); <span class="keyword">end</span>
0078 <span class="keyword">end</span>
0079 <span class="keyword">if</span> U2dim
0080   [dim,nop] = size(U2);
0081   <span class="keyword">if</span> (U2dim~=dim) error(<span class="string">'[ srukf ] Exogenous input U2 dimension differs from InferenceDS.U2dim'</span>); <span class="keyword">end</span>
0082   <span class="keyword">if</span> (dim &amp; (NOV~=nop)) error(<span class="string">'[ srukf ] Number of observation vectors and number of exogenous input U2 vectors do not agree!'</span>); <span class="keyword">end</span>
0083 <span class="keyword">end</span>
0084 
0085 <span class="comment">%--------------------------------------------------------------------------------------------------------------</span>
0086 
0087 <span class="comment">% setup buffer</span>
0088 xh   = zeros(Xdim,NOV);
0089 xh_  = zeros(Xdim,NOV);
0090 yh_  = zeros(Odim,NOV);
0091 inov = zeros(Odim,NOV);
0092 
0093 <span class="comment">% Get UKF scaling parameters</span>
0094 alpha = InferenceDS.spkfParams(1);
0095 beta  = InferenceDS.spkfParams(2);
0096 kappa = InferenceDS.spkfParams(3);
0097 
0098 <span class="comment">% Get index vectors for any of the state or observation vector components that are angular quantities</span>
0099 <span class="comment">% which have discontinuities at +- Pi radians ?</span>
0100 
0101 sA_IdxVec = InferenceDS.stateAngleCompIdxVec;
0102 oA_IdxVec = InferenceDS.obsAngleCompIdxVec;
0103 
0104 
0105 <span class="keyword">switch</span>  InferenceDS.inftype
0106 
0107 <span class="comment">%======================================= PARAMETER ESTIMATION VERSION ===========================================</span>
0108 <span class="keyword">case</span> <span class="string">'parameter'</span>
0109 
0110     L = Xdim+Ndim;                                  <span class="comment">% augmented state dimension</span>
0111     nsp = 2*L+1;                                    <span class="comment">% number of sigma-points</span>
0112     kappa = alpha^2*(L+kappa)-L;                    <span class="comment">% compound scaling parameter</span>
0113 
0114     W = [kappa 0.5 0]/(L+kappa);                    <span class="comment">% sigma-point weights</span>
0115     W(3) = W(1) + (1-alpha^2) + beta;
0116 
0117     sqrtW = W;
0118     possitive_W3 = (W(3) &gt; 0);                      <span class="comment">% is zero'th covariance weight possitive?</span>
0119     sqrtW(1:2) = sqrt(W(1:2));                      <span class="comment">% square root weights</span>
0120     sqrtW(3) = sqrt(abs(W(3)));
0121 
0122     Sqrt_L_plus_kappa = sqrt(L+kappa);
0123 
0124     Zeros_Xdim_X_Ndim     = zeros(Xdim,Ndim);
0125     Zeros_Ndim_X_Xdim     = zeros(Ndim,Xdim);
0126 
0127     Sv = pNoise.cov;
0128     dv = diag(Sv);
0129     Sn = oNoise.cov;
0130     mu_n = oNoise.mu;
0131 
0132     Sx = Sstate;
0133 
0134     <span class="comment">%---  Loop over all input vectors ---</span>
0135     <span class="keyword">for</span> i=1:NOV,
0136 
0137         UU2 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U2(:,i),nsp);
0138 
0139         <span class="comment">%------------------------------------------------------</span>
0140         <span class="comment">% TIME UPDATE</span>
0141 
0142         xh_(:,i) = state;
0143 
0144         <span class="keyword">if</span> pNoise.adaptMethod
0145         <span class="comment">%--------------------------------------------</span>
0146             <span class="keyword">switch</span> pNoise.adaptMethod
0147 
0148             <span class="keyword">case</span> <span class="string">'lambda-decay'</span>
0149                 Sx_ = sqrt(pNoise.adaptParams(1))*Sx;
0150 
0151             <span class="keyword">case</span> {<span class="string">'anneal'</span>,<span class="string">'robbins-monro'</span>}
0152                 Sx_ = Sx + Sv;
0153 
0154             <span class="keyword">end</span>
0155         <span class="comment">%---------------------------------------------</span>
0156         <span class="keyword">else</span>
0157             Sx_ = Sx;
0158         <span class="keyword">end</span>
0159 
0160         Z   = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([xh_(:,i); mu_n],nsp);
0161         Sz  = [Sx_ Zeros_Xdim_X_Ndim; Zeros_Ndim_X_Xdim Sn];
0162         sS  = Sqrt_L_plus_kappa * Sz;
0163         Z(:,2:nsp) = Z(:,2:nsp) + [sS -sS];
0164 
0165         Y_ = InferenceDS.hfun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Ndim,:), UU2);
0166 
0167         temp1 = Z(1:Xdim,:) - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(xh_(:,i),nsp);
0168 
0169 
0170         <span class="comment">%-- Calculate predicted observation mean, dealing with angular discontinuities if needed</span>
0171         <span class="keyword">if</span> isempty(oA_IdxVec)
0172             yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
0173             temp2 = Y_ - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(yh_(:,i),nsp);
0174         <span class="keyword">else</span>
0175             obs_pivotA = Y_(oA_IdxVec,1);      <span class="comment">% extract pivot angle</span>
0176             Y_(oA_IdxVec,1) = 0;
0177             Y_(oA_IdxVec,2:end) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,2:end),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(obs_pivotA,nsp-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0178             yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
0179             yh_(oA_IdxVec,i) = 0;
0180             <span class="keyword">for</span> k=2:nsp,
0181                 yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), W(2)*Y_(oA_IdxVec,k));   <span class="comment">% calculate UT mean ... mod 2pi</span>
0182             <span class="keyword">end</span>
0183             oFoo = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(yh_(:,i),nsp);
0184             temp2 = Y_ - oFoo;
0185             temp2(oA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,:), oFoo(oA_IdxVec,:));
0186             yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), obs_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0187         <span class="keyword">end</span>
0188 
0189 
0190         [foo,Sy] = qr((sqrtW(2)*temp2(:,2:nsp))',0);       <span class="comment">% QR update of observation error Cholesky factor. NOTE: here Sy</span>
0191                                                            <span class="comment">% is the UPPER Cholesky factor (Matlab excentricity)</span>
0192 
0193         <span class="keyword">if</span> possitive_W3                                    <span class="comment">% deal with possible negative zero'th covariance weight</span>
0194             Sy = cholupdate(Sy,sqrtW(3)*temp2(:,1),<span class="string">'+'</span>);
0195         <span class="keyword">else</span>
0196             Sy = cholupdate(Sy,sqrtW(3)*temp2(:,1),<span class="string">'-'</span>);   <span class="comment">% NOTE: here Sy  is the UPPER Cholesky factor (Matlab excentricity)</span>
0197         <span class="keyword">end</span>
0198 
0199         Sy = Sy';                                          <span class="comment">% We need the lower triangular Cholesky factor</span>
0200 
0201         Pxy = W(3)*temp1(:,1)*temp2(:,1)' + W(2)*temp1(:,2:nsp)*temp2(:,2:nsp)';
0202 
0203         KG = (Pxy/Sy')/Sy;
0204 
0205 
0206         <span class="keyword">if</span> isempty(InferenceDS.innovation)
0207             inov(:,i) = obs(:,i) - yh_(:,i);
0208         <span class="keyword">else</span>
0209             inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0210         <span class="keyword">end</span>
0211 
0212 
0213         <span class="keyword">if</span> isempty(sA_IdxVec)
0214            xh(:,i) = xh_(:,i) + KG*inov(:,i);
0215         <span class="keyword">else</span>
0216            upd = KG*inov(:,i);
0217            xh(:,i) = xh_(:,i) + upd;
0218            xh(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), upd(sA_IdxVec));
0219         <span class="keyword">end</span>
0220 
0221 
0222 
0223         Sx_ = Sx_';
0224 
0225         cov_update_vectors = KG*Sy;      <span class="comment">% Correct covariance. This is equivalent to :  Px = Px_ - KG*Py*KG';</span>
0226         <span class="keyword">for</span> j=1:Odim
0227             Sx_ = cholupdate(Sx_,cov_update_vectors(:,j),<span class="string">'-'</span>);
0228         <span class="keyword">end</span>
0229 
0230         Sx = Sx_';
0231         state = xh(:,i);
0232 
0233         <span class="keyword">if</span> pNoise.adaptMethod
0234         <span class="comment">%--- update process noise if needed -----------------------</span>
0235             <span class="keyword">switch</span> pNoise.adaptMethod
0236 
0237             <span class="keyword">case</span> <span class="string">'anneal'</span>
0238                 dV = max(pNoise.adaptParams(1)*(dv.^2) , pNoise.adaptParams(2));
0239                 ds = diag(Sx);
0240                 dv = -ds + sqrt(dV + ds.^2);
0241                 Sv = diag(dv);
0242 
0243             <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0244                 nu = 1/pNoise.adaptParams(1);
0245                 dV = (1-nu)*(dv.^2) + nu*diag(KG*(KG*inov*inov')');
0246                 ds = diag(Sx);
0247                 dv = -ds + sqrt(dV + ds.^2);
0248                 Sv = diag(dv);
0249                 pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0250 
0251             <span class="keyword">otherwise</span>
0252                 error(<span class="string">' [ srukf ]  Process noise update method not allowed.'</span>);
0253 
0254             <span class="keyword">end</span>
0255 
0256             pNoise.cov = Sv;
0257         <span class="comment">%-----------------------------------------------------------</span>
0258         <span class="keyword">end</span>
0259 
0260     <span class="keyword">end</span>   <span class="comment">%... loop over all input vectors</span>
0261 
0262 
0263 
0264 <span class="keyword">otherwise</span>
0265 <span class="comment">%===================================== STATE &amp; JOINT ESTIMATION VERSION ===================================================</span>
0266 
0267     L = Xdim + Vdim + Ndim;                                   <span class="comment">% augmented state dimension</span>
0268     nsp = 2*L+1;                                              <span class="comment">% number of sigma-points</span>
0269     kappa = alpha^2*(L+kappa)-L;                              <span class="comment">% compound scaling parameter</span>
0270 
0271     W = [kappa 0.5 0]/(L+kappa);                              <span class="comment">% sigma-point weights</span>
0272     W(3) = W(1) + (1-alpha^2) + beta;
0273 
0274     sqrtW = W;
0275     possitive_W3 = (W(3) &gt; 0);                                <span class="comment">% is zero'th covariance weight possitive?</span>
0276     sqrtW(1:2) = sqrt(W(1:2));                                <span class="comment">% square root weights</span>
0277     sqrtW(3) = sqrt(abs(W(3)));
0278 
0279     Sqrt_L_plus_kappa = sqrt(L+kappa);
0280 
0281     Zeros_Xdim_X_Vdim     = zeros(Xdim,Vdim);
0282     Zeros_Vdim_X_Xdim     = zeros(Vdim,Xdim);
0283     Zeros_XdimVdim_X_Ndim = zeros(Xdim+Vdim,Ndim);
0284     Zeros_Ndim_X_XdimVdim = zeros(Ndim,Xdim+Vdim);
0285 
0286     Sx = Sstate;
0287     Sv = pNoise.cov;         <span class="comment">% get process noise covariance Cholesky factor</span>
0288     Sn = oNoise.cov;         <span class="comment">% get observation noise covariance Cholesky factor</span>
0289     mu_v = pNoise.mu;      <span class="comment">% get process noise mean</span>
0290     mu_n = oNoise.mu;      <span class="comment">% get measurement noise mean</span>
0291 
0292 
0293     <span class="keyword">if</span> (U1dim==0), UU1=zeros(0,nsp); <span class="keyword">end</span>
0294     <span class="keyword">if</span> (U2dim==0), UU2=zeros(0,nsp); <span class="keyword">end</span>
0295 
0296 
0297     <span class="comment">% if process noise adaptation for joint estimation</span>
0298     <span class="keyword">if</span> pNoise.adaptMethod
0299         <span class="keyword">switch</span> InferenceDS.inftype
0300           <span class="keyword">case</span> <span class="string">'joint'</span>
0301             idx = pNoise.idxArr(<span class="keyword">end</span>,:);     <span class="comment">% get indeces of parameter block of combo-gaussian noise source</span>
0302             ind1 = idx(1);                  <span class="comment">% beginning index of parameter section</span>
0303             ind2 = idx(2);                  <span class="comment">% ending index of parameter section</span>
0304             paramdim = ind2-ind1+1;         <span class="comment">% infer parameter vector length</span>
0305             dv = diag(Sv);                  <span class="comment">% grab diagonal</span>
0306             dv = dv(ind1:ind2);             <span class="comment">% extract the part of the diagonal that relates to the 'parameter section'</span>
0307           <span class="keyword">case</span> <span class="string">'state'</span>
0308             ind1 = 1;
0309             ind2 = Xdim;
0310             paramdim = Xdim;
0311             dv = diag(Sv);
0312         <span class="keyword">end</span>
0313     <span class="keyword">end</span>
0314 
0315 
0316     <span class="comment">%--- Loop over all input vectors -----------------------------------</span>
0317     <span class="keyword">for</span> i=1:NOV,
0318 
0319         <span class="keyword">if</span> U1dim, UU1 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U1(:,i),nsp); <span class="keyword">end</span>
0320         <span class="keyword">if</span> U2dim, UU2 = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(U2(:,i),nsp); <span class="keyword">end</span>
0321 
0322         <span class="comment">%-----------------------------------------</span>
0323         <span class="comment">% TIME UPDATE</span>
0324 
0325         Z   = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>([state; mu_v; mu_n],nsp);
0326         Zm  = Z;                                         <span class="comment">% copy needed for possible angle components section</span>
0327         SzT = [Sx Zeros_Xdim_X_Vdim; Zeros_Vdim_X_Xdim Sv];
0328         Sz  = [SzT Zeros_XdimVdim_X_Ndim; Zeros_Ndim_X_XdimVdim Sn];
0329         sSz = Sqrt_L_plus_kappa * Sz;
0330         sSzM = [sSz -sSz];
0331         Z(:,2:nsp) = Z(:,2:nsp) + sSzM;
0332 
0333         <span class="comment">%-- Calculate predicted state mean, dealing with angular discontinuities if needed</span>
0334         <span class="keyword">if</span> isempty(sA_IdxVec)
0335             X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1);  <span class="comment">% propagate sigma-points through process model</span>
0336             X_bps = X_;
0337             xh_(:,i) = W(1)*X_(:,1) + W(2)*sum(X_(:,2:nsp),2);
0338             temp1 = X_ - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(xh_(:,i),nsp);
0339         <span class="keyword">else</span>
0340             Z(sA_IdxVec,2:nsp) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(Zm(sA_IdxVec,2:nsp), sSzM(sA_IdxVec,:));      <span class="comment">% fix sigma-point set for angular components</span>
0341             X_ = InferenceDS.ffun( InferenceDS, Z(1:Xdim,:), Z(Xdim+1:Xdim+Vdim,:), UU1); <span class="comment">% propagate sigma-points through process model</span>
0342             X_bps = X_;
0343             state_pivotA = X_(sA_IdxVec,1);                                <span class="comment">% extract pivot angle</span>
0344             X_(sA_IdxVec,1) = 0;
0345             X_(sA_IdxVec,2:end) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(X_(sA_IdxVec,2:end),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(state_pivotA,nsp-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0346             xh_(:,i) = W(1)*X_(:,1) + W(2)*sum(X_(:,2:nsp),2);
0347             xh_(sA_IdxVec,i) = 0;
0348             <span class="keyword">for</span> k=2:nsp,
0349                 xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), W(2)*X_(sA_IdxVec,k));     <span class="comment">% calculate UT mean ... mod 2pi</span>
0350             <span class="keyword">end</span>
0351             sFoo = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(xh_(:,i),nsp);
0352             temp1 = X_ - sFoo;
0353             temp1(sA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(X_(sA_IdxVec,:), sFoo(sA_IdxVec,:));
0354             xh_(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), state_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0355         <span class="keyword">end</span>
0356 
0357         [foo,Sx_] = qr((sqrtW(2)*temp1(:,2:nsp))',0);      <span class="comment">% QR update of state Cholesky factor. NOTE: here Sx_</span>
0358                                                            <span class="comment">% is the UPPER Cholesky factor (Matlab excentricity)</span>
0359 
0360         <span class="keyword">if</span> possitive_W3                                    <span class="comment">% deal with possible negative zero'th covariance weight</span>
0361             Sx_ = cholupdate(Sx_,sqrtW(3)*temp1(:,1),<span class="string">'+'</span>);
0362         <span class="keyword">else</span>
0363             Sx_ = cholupdate(Sx_,sqrtW(3)*temp1(:,1),<span class="string">'-'</span>); <span class="comment">% NOTE: here Sx_  is the UPPER Cholesky factor (Matlab excentricity)</span>
0364         <span class="keyword">end</span>
0365 
0366 
0367         Y_ = InferenceDS.hfun( InferenceDS, X_bps, Z(Xdim+Vdim+1:Xdim+Vdim+Ndim,:), UU2);
0368 
0369 
0370         <span class="comment">%-- Calculate predicted observation mean, dealing with angular discontinuities if needed</span>
0371         <span class="keyword">if</span> isempty(oA_IdxVec)
0372             yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
0373             temp2 = Y_ - <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(yh_(:,i),nsp);
0374         <span class="keyword">else</span>
0375             obs_pivotA = Y_(oA_IdxVec,1);      <span class="comment">% extract pivot angle</span>
0376             Y_(oA_IdxVec,1) = 0;
0377             Y_(oA_IdxVec,2:end) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,2:end),<a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(obs_pivotA,nsp-1));  <span class="comment">% subtract pivot angle mod 2pi</span>
0378             yh_(:,i) = W(1)*Y_(:,1) + W(2)*sum(Y_(:,2:nsp),2);
0379             yh_(oA_IdxVec,i) = 0;
0380             <span class="keyword">for</span> k=2:nsp,
0381                 yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), W(2)*Y_(oA_IdxVec,k));   <span class="comment">% calculate UT mean ... mod 2pi</span>
0382             <span class="keyword">end</span>
0383             oFoo = <a href="cvecrep.html" class="code" title="function m = cvecrep(v,c)">cvecrep</a>(yh_(:,i),nsp);
0384             temp2 = Y_ - oFoo;
0385             temp2(oA_IdxVec,:) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(Y_(oA_IdxVec,:), oFoo(oA_IdxVec,:));
0386             yh_(oA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(yh_(oA_IdxVec,i), obs_pivotA);  <span class="comment">% add pivot angle back to calculate actual predicted mean</span>
0387         <span class="keyword">end</span>
0388 
0389         [foo,Sy] = qr((sqrtW(2)*temp2(:,2:nsp))',0);       <span class="comment">% QR update of observation error Cholesky factor. NOTE: here Sy</span>
0390                                                            <span class="comment">% is the UPPER Cholesky factor (Matlab excentricity)</span>
0391 
0392         <span class="keyword">if</span> possitive_W3                                    <span class="comment">% deal with possible negative zero'th covariance weight</span>
0393             Sy = cholupdate(Sy,sqrtW(3)*temp2(:,1),<span class="string">'+'</span>);
0394         <span class="keyword">else</span>
0395             Sy = cholupdate(Sy,sqrtW(3)*temp2(:,1),<span class="string">'-'</span>);   <span class="comment">% NOTE: here Sy  is the UPPER Cholesky factor (Matlab excentricity)</span>
0396         <span class="keyword">end</span>
0397 
0398         Sy = Sy';                                          <span class="comment">% We need the lower triangular Cholesky factor</span>
0399 
0400 
0401         <span class="comment">%------------------------------------------------------</span>
0402         <span class="comment">% MEASUREMENT UPDATE</span>
0403 
0404         Pxy = W(3)*temp1(:,1)*temp2(:,1)' + W(2)*temp1(:,2:nsp)*temp2(:,2:nsp)';
0405 
0406         KG = (Pxy/Sy')/Sy;
0407 
0408         <span class="keyword">if</span> isempty(InferenceDS.innovation)
0409             inov(:,i) = obs(:,i) - yh_(:,i);
0410             <span class="keyword">if</span> ~isempty(oA_IdxVec)
0411               inov(oA_IdxVec,i) = <a href="subangle.html" class="code" title="function C = subangle(A, B)">subangle</a>(obs(oA_IdxVec,i), yh_(oA_IdxVec,i));
0412             <span class="keyword">end</span>
0413         <span class="keyword">else</span>
0414             inov(:,i) = InferenceDS.innovation( InferenceDS, obs(:,i), yh_(:,i));  <span class="comment">% inovation (observation error)</span>
0415         <span class="keyword">end</span>
0416 
0417 
0418         <span class="keyword">if</span> isempty(sA_IdxVec)
0419            xh(:,i) = xh_(:,i) + KG*inov(:,i);
0420         <span class="keyword">else</span>
0421            upd = KG*inov(:,i);
0422            xh(:,i) = xh_(:,i) + upd;
0423            xh(sA_IdxVec,i) = <a href="addangle.html" class="code" title="function C = addangle(A, B)">addangle</a>(xh_(sA_IdxVec,i), upd(sA_IdxVec));
0424         <span class="keyword">end</span>
0425 
0426         cov_update_vectors = KG*Sy;      <span class="comment">% Correct covariance. This is equivalent to :  Px = Px_ - KG*Py*KG';</span>
0427         <span class="keyword">for</span> j=1:Odim
0428             Sx_ = cholupdate(Sx_,cov_update_vectors(:,j),<span class="string">'-'</span>);
0429         <span class="keyword">end</span>
0430         Sx = Sx_';
0431 
0432         state = xh(:,i);
0433 
0434 
0435         <span class="keyword">if</span> pNoise.adaptMethod
0436         <span class="comment">%--- update process noise if needed for joint estimation ----------------------</span>
0437             <span class="keyword">switch</span> pNoise.adaptMethod
0438 
0439             <span class="keyword">case</span> <span class="string">'anneal'</span>
0440                 dv = sqrt(max(pNoise.adaptParams(1)*(dv.^2) , pNoise.adaptParams(2)));
0441                 Sv(ind1:ind2,ind1:ind2) = diag(dv);
0442 
0443             <span class="keyword">case</span> <span class="string">'robbins-monro'</span>
0444                 nu = 1/pNoise.adaptParams(1);
0445                 subKG = KG(end-paramdim+1:<span class="keyword">end</span>,:);
0446                 dv = sqrt((1-nu)*(dv.^2) + nu*diag(subKG*(subKG*inov*inov')'));
0447                 Sv(ind1:ind2,ind1:ind2) = diag(dv);
0448                 pNoise.adaptParams(1) = min(pNoise.adaptParams(1)+1, pNoise.adaptParams(2));
0449 
0450             <span class="keyword">otherwise</span>
0451                 error(<span class="string">' [ srukf ]  Process noise update method not allowed.'</span>);
0452 
0453             <span class="keyword">end</span>
0454 
0455             pNoise.cov = Sv;
0456         <span class="comment">%-----------------------------------------------------------</span>
0457         <span class="keyword">end</span>
0458 
0459     <span class="keyword">end</span>   <span class="comment">%... loop over all input vectors</span>
0460 
0461 
0462 <span class="comment">%====================================================================================================================</span>
0463 <span class="keyword">end</span>
0464 
0465 
0466 <span class="keyword">if</span> (nargout&gt;4),
0467     InternalVariablesDS.xh_   = xh_;
0468     InternalVariablesDS.Sx_   = Sx_;
0469     InternalVariablesDS.yh_   = yh_;
0470     InternalVariablesDS.inov  = inov;
0471     InternalVariablesDS.Sinov = Sy;
0472     InternalVariablesDS.KG    = KG;
0473 <span class="keyword">end</span></pre></div>
<hr><address>Generated on Tue 26-Sep-2006 10:36:21 by <strong><a href="http://www.artefact.tk/software/matlab/m2html/">m2html</a></strong> &copy; 2003</address>
</body>
</html>